#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     touchSensor,    sensorTouch)
#pragma config(Sensor, S3,     IRSensor,       sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorLeft,     tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorRight,    tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorArm,      tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    fingerLeft,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    fingerRight,          tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servoWrist,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servoIR,              tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
#include "C:\ftc\drivers\2.3\drivers\HTIRS2-driver.h"

task main ()
{
    waitForStart(); //wait until FCS tells us to go

    // zero encoders
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    nMotorEncoder[motorArm] = 0;
    //close hand
    servo[fingerLeft] = 100;
    servo[fingerRight] = 160;
    servo[servoWrist] = 235;

    servo[servoIR] = 129; //put IR servo in spot so IR sensor is dir 5 is forward
    wait1Msec(1000);

    float wheelDiameter = 3.804;

    //create variables to store sensor outputs and set it to AC 1200Hz
    int dirAC = 1;
    tHTIRS2DSPMode mode = DSP_1200; //from robotc forums

    dirAC = HTIRS2readACDir(IRSensor); //from robotc forums
    writeDebugStreamLine("dir=%d", dirAC);


    if(dirAC == 5)//beacon on left peg
    {
        while(nMotorEncoder[motorLeft] < (1440 * 49 /(wheelDiameter * PI))) //go straight
        {
            motor[motorLeft] = 50;
            motor[motorRight] = 50;
        }
        nMotorEncoder[motorLeft] = 0;
        //turn right
        while(nMotorEncoder[motorLeft] < (1440 * 0.73))
        {
            motor[motorLeft] = 50;
            motor[motorRight] = -50;
        }
        nMotorEncoder[motorLeft] = 0;
        motor[motorLeft] = 0;
        motor[motorRight] = 0;
        servo[servoIR] = 90;// put IR servo in a spot where dir 6 of the IR sensor is forward
        wait10Msec(100);
        long encTarget = 0;
        int increment = 0;
        while(dirAC != 6 && increment < 10)
        {
            dirAC = HTIRS2readACDir(IRSensor); //read value
            writeDebugStreamLine("dir=%d", dirAC);

            int val = dirAC - 6;//power adjusting number
            increment = increment + 1;
            writeDebugStreamLine("inc=%d", increment);
            /*encTarget++;
            while(nMotorEncoder[motorLeft] < encTarget)
            {
            motor[motorLeft] = 50;
            motor[motorRight] = -50;
            }*/

            motor[motorLeft] = 30 * -val * 0.9 * increment;
            motor[motorRight] = 30 * val * 0.9 * increment;
        }
        motor[motorLeft] = 0;
        motor[motorRight] = 0;
        wait10Msec(20);
        while(nMotorEncoder[motorArm] < (1440 * 4.1)) //put arm up to peg height
        {
            motor[motorArm] = 100;
        }

        motor[motorArm] = 0; //stop arm

        while(nMotorEncoder[motorLeft] < (1440 * 12 /(wheelDiameter * PI))) //go straight
        {
            motor[motorLeft] = 50;
            motor[motorRight] = 50;
        }
        motor[motorLeft] = 0;
        motor[motorRight] = 0;
        servo[servoWrist] = 127; //move wrist forward
        wait1Msec(500);

        servo[fingerLeft] = 200; //open hand
        servo[fingerRight] = 50;
        wait1Msec(250);

        motor[motorLeft] = -50; //back up
        motor[motorRight] = -50;
        wait1Msec(100);
    }
    else if(dirAC == 4)//beacon on center peg
    {
        while(nMotorEncoder[motorLeft] < (1440 * 24 /(wheelDiameter * PI))) //go straight
        {
            motor[motorLeft] = 50;
            motor[motorRight] = 50;
        }
        nMotorEncoder[motorLeft] = 0;
        //turn right
        while(nMotorEncoder[motorLeft] < (1440 * 0.73))
        {
            motor[motorLeft] = 50;
            motor[motorRight] = -50;
        }
        nMotorEncoder[motorLeft] = 0;
        motor[motorLeft] = 0;
        motor[motorRight] = 0;
        servo[servoIR] = 90;// put IR servo in a spot where dir 6 of the IR sensor is forward
        wait10Msec(100);
        long encTarget = 0;
        int increment = 0;
        while(dirAC != 6 && increment < 10)
        {
            dirAC = HTIRS2readACDir(IRSensor); //read value
            writeDebugStreamLine("dir=%d", dirAC);

            int val = dirAC - 6;//power adjusting number
            increment = increment + 1;
            writeDebugStreamLine("inc=%d", increment);
            /*encTarget++;
            while(nMotorEncoder[motorLeft] < encTarget)
            {
            motor[motorLeft] = 50;
            motor[motorRight] = -50;
            }*/

            motor[motorLeft] = 30 * -val * 0.9 * increment;
            motor[motorRight] = 30 * val * 0.9 * increment;
        }
        motor[motorLeft] = 0;
        motor[motorRight] = 0;
        wait10Msec(20);
        while(nMotorEncoder[motorArm] < (1440 * 4.1)) //put arm up to peg height
        {
            motor[motorArm] = 100;
        }

        motor[motorArm] = 0; //stop arm

        while(nMotorEncoder[motorLeft] < (1440 * 30 /(wheelDiameter * PI))) //go straight
        {
            motor[motorLeft] = 50;
            motor[motorRight] = 50;
        }
        motor[motorLeft] = 0;
        motor[motorRight] = 0;
    }
    else if(dirAC == 3)// beacon on right peg
    {
        while(nMotorEncoder[motorLeft] < (1440 * 1 /(wheelDiameter * PI))) //go straight
        {
            motor[motorLeft] = 50;
            motor[motorRight] = 50;
        }
        nMotorEncoder[motorLeft] = 0;
        //turn right
        while(nMotorEncoder[motorLeft] < (1440 * 0.73))
        {
            motor[motorLeft] = 50;
            motor[motorRight] = -50;
        }
        nMotorEncoder[motorLeft] = 0;
        motor[motorLeft] = 0;
        motor[motorRight] = 0;
        servo[servoIR] = 90;// put IR servo in a spot where dir 6 of the IR sensor is forward
        wait10Msec(100);
        long encTarget = 0;
        int increment = 0;
        while(dirAC != 6 && increment < 10)
        {
            dirAC = HTIRS2readACDir(IRSensor); //read value
            writeDebugStreamLine("dir=%d", dirAC);

            int val = dirAC - 6;//power adjusting number
            increment = increment + 1;
            writeDebugStreamLine("inc=%d", increment);
            /*encTarget++;
            while(nMotorEncoder[motorLeft] < encTarget)
            {
            motor[motorLeft] = 50;
            motor[motorRight] = -50;
            }*/

            motor[motorLeft] = 30 * -val * 0.9 * increment;
            motor[motorRight] = 30 * val * 0.9 * increment;
        }
        motor[motorLeft] = 0;
        motor[motorRight] = 0;
        wait10Msec(20);
        while(nMotorEncoder[motorArm] < (1440 * 4.1)) //put arm up to peg height
        {
            motor[motorArm] = 100;
        }

        motor[motorArm] = 0; //stop arm

        while(nMotorEncoder[motorLeft] < (1440 * 46 /(wheelDiameter * PI))) //go straight
        {
            motor[motorLeft] = 50;
            motor[motorRight] = 50;
        }
        motor[motorLeft] = 0;
        motor[motorRight] = 0;
    }

    /*servo[servoIR] = 90;// put IR servo in a spot where dir 6 of the IR sensor is forward
    wait1Msec(500);
    int increment = 0;
    while(dirAC != 0 && increment < 10) //go forwards using IR until it can't read a value
    {
    dirAC = HTIRS2readACDir(IRSensor); //read value
    writeDebugStreamLine("dir=%d", dirAC);

    int val = dirAC - 6;//power adjusting number
    increment++;

    if ( val == 0 )// if robot is facing beacon drive straight
    {
    motor[motorLeft] = 50;
    motor[motorRight] = 50;
    }
    else // turn so robot is facing beacon
    {
    motor[motorLeft] = 30 * -val * 0.9 * increment;
    motor[motorRight] = 30 * val * 0.9 * increment;
    }

    //close hand
    servo[fingerRight] = 160;
    servo[fingerLeft] = 100;

    wait10Msec(2);
    }*/

    servo[servoWrist] = 127; //move wrist forward
    wait1Msec(500);

    servo[fingerLeft] = 200; //open hand
    servo[fingerRight] = 50;
    wait1Msec(250);

    while(nMotorEncoder[motorLeft] > (1440 * 24/(wheelDiameter * PI))) //back up
    {
        motor[motorLeft] = -50;
        motor[motorRight] = -50;
    }
    motor[motorLeft] = 0;//stop
    motor[motorRight] = 0;
    while(SensorValue(touchSensor) == 0) //put arm down
    {
        motor[motorArm] = -100;
    }
    motor[motorArm] = 0;
}
